%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                              Midterm 1                                  %
%                       EE556: Dr. fred harris                            %
%                     Author: Michael Spinali                             %
%                             813488956                                   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Create Fresh Environment
close all
clear all
clc

% Definitions
ff = [697 770 852 941 1209 1336 1477 1633];      % Freq. Vector (Hz)
aa = [1 0.5 0.25 0.125 1 0.5 0.25 0.125];        % Amp. Vector
fs1 = 9600;                                      % Sampling Freq 1    
fs2 = 4800;                                      % Sampling Freq 2
fs3 = 1200;                                      % Sampling Freq 3  
n = 0:1:20E3-1;                                  % Time Vector (Samples)
Test = aa*(sin(2*pi*(ff'*n)/fs1));
NFFT = 2048;

if(rem(NFFT,2) == 0)
  % N is even
  f = linspace(-fs1/2, (fs1/2) - (fs1/NFFT), NFFT);
else
  % N is odd
  f = linspace(-fs1/2, (fs1/2), NFFT);
end

[bb,aa] = ellip(6,0.1,65,1633/(fs1/2));
[sos,g]=tf2sos(bb,aa);
% sos 
%     1       1.6901            1            1      -1.0526      0.33204
%     1      0.54846            1            1     -0.90685      0.57825
%     1     0.030389            1            1     -0.82201      0.86056

b1_1 = sos(1,2);
b1_2 = sos(1,3);
a1_1 = sos(1,5);
a1_2 = sos(1,6);
b1_0=(1+a1_1+a1_2)/(1+b1_1+b1_2);               % stage 1 scale factor

b2_1 = sos(2,2);
b2_2 = sos(2,3);
a2_1 = sos(2,5);
a2_2 = sos(2,6);
b2_0=(1+a2_1+a2_2)/(1+b2_1+b2_2);               % stage 2 scale factor

b3_1 = sos(3,2);
b3_2 = sos(3,3);
a3_1 = sos(3,5);
a3_2 = sos(3,6);
b3_0=(1+a3_1+a3_2)/(1+b3_1+b3_2);               % stage 3 scale factor


w1_1=0;
w1_2=0;
w2_1=0;
w2_2=0;
w3_1=0;
w3_2=0;

x0=[1 zeros(1,100)];
y1=zeros(1,100);
for i=1:100
%   Stage 1
   w1_0=x0(i) - w1_1*a1_1 - w1_2*a1_2;      % Feedback term
   x1=(w1_0 + w1_1*b1_1 + w1_2*b1_2)*b1_0;  % Scaled Feed Forward
   w1_2 = w1_1;                             % Shift w1_1 to w1_2
   w1_1 = w1_0;                             % Shift w1_0 to w1_1
%	Stage 2
   w2_0=x1 - w2_1*a2_1 - w2_2*a2_2;         % Feedback term
   x2=(w2_0 + w2_1*b2_1 + w2_2*b2_2)*b2_0;  % Scaled Feed Forward
   w2_2 = w2_1;                             % Shift w2_1 to w2_2
   w2_1 = w2_0;                             % Shift w2_0 to w2_1
%	Stage 3
   w3_0=x2 - w3_1*a3_1 - w3_2*a3_2;      % Feedback term
   x3=(w3_0 + w3_1*b3_1 + w3_2*b3_2)*b3_0;  % Scaled Feed Forward
   w3_2 = w3_1;                             % Shift w3_1 to w3_2
   w3_1 = w3_0;                             % Shift w3_0 to w3_1
   
   y1(i)=x3;                                % Output
end

figure(1)
subplot(2,1,1)
plot(y1)
grid on
axis([0 100 -0.2 0.35])
title('Filter Impulse response')

subplot(2,1,2)
plot(f,fftshift(20*log10(abs(fft(y1,NFFT)))))
grid on
axis([0 4800 -100 5])
title('Frequency Response')


% fxn=fftshift(20*log10(abs(fft(Test,NFFT))));
% plot(f,fxn)
% Test = bsxfun(@times,sin(2*pi*(ff'*n)/Fs),aa');
